Marker Based Bundle Adjustment

通常视觉slam中landmark为一个世界系下的点,可以参数化为一个向量$P = [x,y,z]^T$ 或者参数化为第一次观测到的图像坐标加一维逆深度。

对于使用marker的slam来说,通常的做法并不可取。对于一个marker来说,四个特征点的位置是具有特殊的约束(四个坐标点之间的距离固定,并且在一个平面内)。因此在这里将marker作为一个整体参数化为一个三维空间下的旋转加平移。

Bundle Adjustment

相关定义如下:

  • marker特征点在marker坐标系为$P^m$
  • marker 在相机坐标系坐标为$[t^c_m, R^c_m]$
  • 相机在世界坐标系坐标为$[t^w_c, R^w_c]$

cost function

cost function定义为相机系下的marker特征点投影的预测值和观测值做减法,如下:

其中$\pi()$ 为投影函数。

jacobian

jacobian为costfunction对求解变量的导数,可以根据链式法则分解为两部分:

归一化相机坐标$p=[u, v]^T$相机坐标系点$P^c = [x,y,z]^T$

相机坐标系点$P^c=[x,y,z]^T$marker的平移和旋转

Demo

此demo读取一帧图像,首先进行PNP计算当前相机位姿,再通过BundleAdjustment优化。图中黄色为PNP的结果叠加上噪声后重投影,绿色为BundleAdjustment优化的结果重投影。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
class MarkerProjectionCostfunction : public ceres::SizedCostFunction<ceres::DYNAMIC, 7, 7>
{
public:
MarkerProjectionCostfunction(const std::vector<cv::Point2f>& obs,
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> corners, const Eigen::Matrix2d& information)
: obs_(obs)
, corners_(corners)
{
set_num_residuals(corners.size() * 2);
}
virtual ~MarkerProjectionCostfunction() {}
public:
virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
{
//Eigen::Vector3d twb(parameters[0][0], parameters[0][1], parameters[0][2]);
//Eigen::Quaterniond rwb(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);
Eigen::Vector3d tbm(parameters[1][0], parameters[1][1], parameters[1][2]);
Eigen::Quaterniond rbm(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);

Eigen::Map<Eigen::VectorXd> residual(residuals, num_residuals());
std::vector<Eigen::Matrix<double, 2, 3>> reduces;
int size = corners_.size();
for (int pos = 0; pos < size; pos++){
auto& pm = corners_[pos];
Eigen::Vector3d pb = rbm * pm + tbm;
Eigen::Matrix<double, 2, 3> reduce;
reduce << 1. / pb.z(), 0, -pb.x() / (pb.z() * pb.z()),
0, 1. / pb.z(), -pb.y() / (pb.z() * pb.z());
reduces.push_back(reduce);
residual.segment(pos * 2, 2) = (pb / pb.z()).head<2>() - Eigen::Vector2d(obs_[pos].x, obs_[pos].y);
}

if (jacobians) {
if (jacobians[0]) {
Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobian(jacobians[0], num_residuals(), 7);
jacobian.setZero();
}
if (jacobians[1]) {
Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobian(jacobians[1], num_residuals(), 7);
for (int pos = 0; pos < size; pos++) {
Eigen::Matrix<double, 3, 6> jaco;
jaco.leftCols<3>() = Eigen::Matrix3d::Identity();
jaco.rightCols<3>() = -1. * rbm.toRotationMatrix() * Sophus::SO3d::hat(corners_[pos]);

jacobian.block<2, 7>(pos * 2, 0).leftCols<6>() = reduces[pos] * jaco;
jacobian.block<2, 7>(pos * 2, 0).rightCols<1>().setZero();
}
}
}
return true;
}
public:
std::vector<cv::Point2f> obs_;
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> corners_;
};

bool bundleAdjuster()
{
// read images
cv::Mat image = cv::imread("image.png", cv::IMREAD_GRAYSCALE);
if (image.empty()) {
return false;
}
// debug
cv::Mat output = image.clone();
cv::cvtColor(output, output, CV_GRAY2RGB);

// detect
auto detector = boost::make_shared<AprilTagDetector>("tag36h11", 0.);
Markers markers;
detector->find(image, markers);
if (markers.empty()) {
return false;
}
for (auto& marker : markers) {
cv::line(output, marker.corners[0], marker.corners[1], cv::Scalar(0, 255, 0));
cv::line(output, marker.corners[1], marker.corners[2], cv::Scalar(0, 0, 255));
cv::line(output, marker.corners[2], marker.corners[3], cv::Scalar(0, 0, 255));
cv::line(output, marker.corners[3], marker.corners[0], cv::Scalar(255, 0, 0));
std::string label = cv::format("#%d", marker.id);
cv::putText(output, label, marker.center, cv::FONT_HERSHEY_COMPLEX_SMALL, 1., cv::Scalar(255, 0, 0), 1);
}
// undistort
double fx = 442.743649;
double s = 0;
double cx = 301.858989;
double fy = 443.371918;
double cy = 257.307880;

double k1 = -0.307555;
double k2 = 0.086253;
double k3 = 0;
double p1 = 0.000436;
double p2 = -0.000926;

cv::Mat intrinsic_matrix = cv::Mat::eye(3, 3, CV_64F);
intrinsic_matrix.at<double>(0, 0)/* fx */ = fx;
intrinsic_matrix.at<double>(0, 1)/* s */ = s;
intrinsic_matrix.at<double>(0, 2)/* cx */ = cx;
intrinsic_matrix.at<double>(1, 1)/* fy */ = fy;
intrinsic_matrix.at<double>(1, 2)/* cy */ = cy;

cv::Mat distortion_coeffs = cv::Mat::zeros(5, 1, CV_64F);
distortion_coeffs.at<double>(0, 0) = k1;
distortion_coeffs.at<double>(1, 0) = k2;
distortion_coeffs.at<double>(2, 0) = p1;
distortion_coeffs.at<double>(3, 0) = p2;
distortion_coeffs.at<double>(4, 0) = k3;

std::vector<cv::Point2f> observation;
cv::undistortPoints(markers[0].corners, observation, intrinsic_matrix, distortion_coeffs, cv::Mat(), intrinsic_matrix);


for (auto &point : observation) {
//point.x += (3. * (float)((std::rand() % 10000) - 5000) / 5000.);
//point.y += (3. * (float)((std::rand() % 10000) - 5000) / 5000.);
point.x = (point.x - cx) / fx;
point.y = (point.y - cy) / fy;
}

{
std::vector<cv::Point2f> reprojects;
std::vector<cv::Point3f> objects;
cv::convertPointsToHomogeneous(observation, objects);
cv::projectPoints(objects, cv::Vec3f(0., 0., 0.), cv::Vec3f(0., 0., 0.), intrinsic_matrix, distortion_coeffs, reprojects);
}
// pnp
Eigen::Matrix4d twb = Eigen::Matrix4d::Identity();
Eigen::Matrix4d tbm = Eigen::Matrix4d::Identity();
{
std::vector<cv::Point3f> objects;
objects.push_back(cv::Point3f(-0.08, -0.08, 0.));
objects.push_back(cv::Point3f(0.08, -0.08, 0.));
objects.push_back(cv::Point3f(0.08, 0.08, 0.));
objects.push_back(cv::Point3f(-0.08, 0.08, 0.));
cv::Mat r, t, rot;
cv::Mat intrinsic = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
if (!cv::solvePnP(objects, observation, intrinsic, cv::Mat(), r, t)) {
return false;
}
cv::Rodrigues(r, rot);
{
cv::Mat noise = (cv::Mat_<double>(3, 1) <<
(0.5 * (float)((std::rand() % 10000) - 5000) / 5000.),
(0.5 * (float)((std::rand() % 10000) - 5000) / 5000.),
(0.5 * (float)((std::rand() % 10000) - 5000) / 5000.));
t += noise;
}
{
cv::Mat noise = (cv::Mat_<double>(3, 1) <<
(0.5 * (float)((std::rand() % 10000) - 5000) / 5000.),
(0.5 * (float)((std::rand() % 10000) - 5000) / 5000.),
(0.5 * (float)((std::rand() % 10000) - 5000) / 5000.));
r += noise;
}

cv::Mat trans = cv::Mat::eye(4, 4, CV_32F);
rot.copyTo(trans.rowRange(0, 3).colRange(0, 3));
t.copyTo(trans.rowRange(0, 3).colRange(3, 4));
cv::cv2eigen(trans, tbm);
std::cout << "tbm" << std::endl << tbm << std::endl;

std::vector<cv::Point2f> reprojects;
cv::projectPoints(objects, r, t, intrinsic_matrix, distortion_coeffs, reprojects);
std::cout << "reprojection: " << std::endl;
for (int pos = 0; pos < reprojects.size(); pos ++) {
auto& point = reprojects[pos];
auto& origin = markers[0].corners[pos];
std::printf("reprojection: (%8.5f,%8.5f)->(%8.5f,%8.5f) = (%8.5f,%8.5f).\n", origin.x, origin.y, point.x, point.y, origin.x-point.x, origin.y-point.y);
cv::circle(output, point, 2, cv::Scalar(0, 255, 255), -1);
}
}
// bundle adjuster
{
ceres::Problem problem;
ceres::LossFunction* loss = new ceres::HuberLoss(1.0);

std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> corners;
corners.push_back(Eigen::Vector3d(-0.08, -0.08, 0.));
corners.push_back(Eigen::Vector3d(0.08, -0.08, 0.));
corners.push_back(Eigen::Vector3d(0.08, 0.08, 0.));
corners.push_back(Eigen::Vector3d(-0.08, 0.08, 0.));

Eigen::Matrix<double, 7, 1, Eigen::DontAlign> vtwb;
vtwb.segment(0, 3) = twb.block<3, 1>(0, 3);
vtwb.segment(3, 4) = Eigen::Quaterniond(twb.block<3, 3>(0, 0)).coeffs();
problem.AddParameterBlock(vtwb.data(), 7, new SpecialEuclideanGroupPlus());
problem.SetParameterBlockConstant(vtwb.data());

Eigen::Matrix<double, 7, 1, Eigen::DontAlign> vtbm;
vtbm.segment(0, 3) = tbm.block<3, 1>(0, 3);
vtbm.segment(3, 4) = Eigen::Quaterniond(tbm.block<3, 3>(0, 0)).coeffs();
problem.AddParameterBlock(vtbm.data(), 7, new SpecialEuclideanGroupPlus());

auto factor = new MarkerProjectionCostfunction(observation, corners, Eigen::Matrix2d::Identity());
problem.AddResidualBlock(factor, loss, vtwb.data(), vtbm.data());

ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.trust_region_strategy_type = ceres::DOGLEG;
options.max_num_iterations = 10000;
options.max_solver_time_in_seconds = 1.;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::printf("%s\n", summary.FullReport().c_str());
/**/

Eigen::Matrix4d ttbm = Eigen::Matrix4d::Identity();
ttbm.block<3, 3>(0, 0) = Eigen::Quaterniond(vtbm.segment(3, 4).data()).normalized().toRotationMatrix();
ttbm.block<3, 1>(0, 3) = vtbm.segment(0, 3);
std::cout << "ttbm" << std::endl << ttbm << std::endl;

{
std::vector<cv::Point3f> objects;
for (auto &corner : corners) {
Eigen::Vector3d object = ttbm.block<3, 3>(0, 0) * corner + ttbm.block<3, 1>(0, 3);
objects.push_back(cv::Point3f(object.x(), object.y(), object.z()));
}

std::vector<cv::Point2f> reprojects;
cv::projectPoints(objects, cv::Vec3f(0., 0., 0.), cv::Vec3f(0., 0., 0.), intrinsic_matrix, distortion_coeffs, reprojects);
std::cout << "bundle adjuster reprojection: " << std::endl;
for (int pos = 0; pos < reprojects.size(); pos++) {
auto& point = reprojects[pos];
auto& origin = markers[0].corners[pos];
std::printf("reprojection: (%8.5f,%8.5f)->(%8.5f,%8.5f) = (%8.5f,%8.5f).\n", origin.x, origin.y, point.x, point.y, origin.x - point.x, origin.y - point.y);
cv::circle(output, point, 1, cv::Scalar(0, 255, 0), -1);
}
}
}

cv::imshow("view", output); cv::waitKey(0);
return true;
}

Reference

[1] <视觉slam十四讲>
[2] https://blog.csdn.net/heyijia0327/article/details/60143160